
#include "stm32f10x_conf.h"
#include "stm32lib/stm32f10x_gpio.h"

#include "common.h"
#include "magnetic_encoder.h"

#define ROT_CS_PIN         GPIO_Pin_7
#define ROT_DCLK_PIN       GPIO_Pin_13
#define ROT_DIO_PIN        GPIO_Pin_1

#define ROT_CS_PORT        GPIOA
#define ROT_DCLK_PORT      GPIOC
#define ROT_DIO_PORT       GPIOB

/*
#define ROT_CS_PIN         GPIO_Pin_8
#define ROT_DCLK_PIN       GPIO_Pin_5
#define ROT_DIO_PIN        GPIO_Pin_1

#define ROT_CS_PORT        GPIOB
#define ROT_DCLK_PORT      GPIOA
#define ROT_DIO_PORT       GPIOC
*/
#define CMD_WRITE_OTP 31
#define CMD_PROG_OTP 25
#define CMD_WRITE_CONFIG_1 23
#define CMD_SET_MT_COUNTER 20
#define CMD_EN_PROG 16
#define CMD_READ_OTP 15
#define CMD_READ_ANA 9
#define CMD_RD_MT_COUNTER 4
#define CMD_RD_ANGLE 0

GPIO_InitTypeDef Dio_GPIO_InitStructure;

MagneticEncoder::MagneticEncoder(void)
{
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);

  GPIO_InitTypeDef GPIO_InitStructure;
  GPIO_INIT_MODE(ROT_CS, GPIO_Mode_Out_PP);
  GPIO_INIT_MODE(ROT_DCLK, GPIO_Mode_Out_PP);
  GPIO_INIT_MODE(ROT_DIO, GPIO_Mode_Out_PP);

  GPIO_RESET(ROT_CS);
  GPIO_RESET(ROT_DCLK);
  GPIO_RESET(ROT_DIO);
}

inline void MagneticEncoder::setDataDirectionInput(void)
{
  GPIO_InitTypeDef GPIO_InitStructure;
  GPIO_INIT_MODE(ROT_DIO, GPIO_Mode_IPU);
}

inline void MagneticEncoder::setDataDirectionOutput(void)
{
  GPIO_InitTypeDef GPIO_InitStructure;
  GPIO_INIT_MODE(ROT_DIO, GPIO_Mode_Out_PP);
}

inline void MagneticEncoder::writeCommand(unsigned int command)
{
  setDataDirectionOutput();

  GPIO_RESET(ROT_CS);
  GPIO_RESET(ROT_DCLK);
  GPIO_RESET(ROT_DIO);

  GPIO_SET(ROT_CS);
  for (int idx = 4; idx >= 0; idx--)
  {
    if (command & (1<<idx))
    {
      GPIO_SET(ROT_DIO);
    }
    else
    {
      GPIO_RESET(ROT_DIO);
    }
    GPIO_SET(ROT_DCLK);
    GPIO_RESET(ROT_DCLK);
  }
}

void MagneticEncoder::write(unsigned int command, unsigned int data)
{
  writeCommand(command);

  for (int idx = 15; idx >= 0; idx--)
  {
    if (data & (1<<idx))
    {
      GPIO_SET(ROT_DIO);
    }
    else
    {
      GPIO_RESET(ROT_DIO);
    }
    GPIO_SET(ROT_DCLK);
    GPIO_RESET(ROT_DCLK);
  }
  GPIO_RESET(ROT_CS);
}

unsigned int MagneticEncoder::read(unsigned int command)
{
  unsigned int read_value = 0;

  writeCommand(command);
  setDataDirectionInput();
  
  for (int idx = 15; idx >= 0; idx--)
  {
    GPIO_SET(ROT_DCLK);

    if (GPIO_GET(ROT_DIO))
    {
      read_value += (unsigned int)(1<<idx);
    }
    GPIO_RESET(ROT_DCLK);
  }
  GPIO_RESET(ROT_CS);

  return read_value ;
}

int MagneticEncoder::getAngle(void)
{
/*
  while (true) {
    GPIO_RESET(ROT_DIO);
    GPIO_RESET(ROT_DCLK);
    GPIO_RESET(ROT_CS);

    GPIO_SET(ROT_DIO);
    GPIO_SET(ROT_DCLK);
    GPIO_SET(ROT_CS);
  }

    GPIO_RESET(ROT_DIO);
    GPIO_RESET(ROT_DCLK);
    GPIO_RESET(ROT_CS);
*/


  int angle;
  angle = (int)(read(CMD_RD_ANGLE) & 0x1FF);
  return angle;
}
